#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
配置管理模块
Configuration Management Module

负责处理机器人的配置参数、校准数据和速度标定
Handles robot configuration parameters, calibration data and speed calibration

作者: YAHBOOM | Gentle Xu
Author: YAHBOOM | Gentle Xu
日期: 2025/09
Date: 2025/09
"""

import time
import random
from typing import Dict, Any, Optional


class ConfigManager:
    """
    配置管理器
    Configuration Manager
    
    管理机器人的全局配置、校准数据和速度标定
    Manages robot global configuration, calibration data and speed calibration
    """
    
    def __init__(self, calibration_distance_cm: float = 28, calibration_steps: int = 10, debug: bool = True):
        """
        初始化配置管理器
        Initialize configuration manager
        
        Args:
            calibration_distance_cm: 校准距离(厘米) Calibration distance in cm
            calibration_steps: 校准步数 Calibration steps
            debug: 调试模式 Debug mode
        """
        self.debug = debug
        
        # 校准数据 Calibration data
        self.calibration_distance_cm = calibration_distance_cm
        self.calibration_steps = calibration_steps
        self.step_distance_cm = calibration_distance_cm / calibration_steps
        self.step_distance_m = self.step_distance_cm / 100.0
        
        # 默认配置 Default configuration
        self.default_config = {
            'step_distance_cm': self.step_distance_cm,
            'step_distance_m': self.step_distance_m,
            'calibration_distance_cm': calibration_distance_cm,
            'calibration_steps': calibration_steps,
            'pause_between_actions_s': 1.0,
            'debug': debug,
            
            # 基础参数 Basic parameters
            'default_steps': 10,             # 默认步数 Default steps
            'default_distance_m': 0.5,       # 默认距离(米) Default distance in meters
            'default_time_s': 2.0,           # 默认时间(秒) Default time in seconds
            'default_speed_ms': 0.25,        # 默认速度(米/秒) Default speed in m/s
            'default_angular_speed_deg_s': 30, # 默认角速度(度/秒) Default angular speed in deg/s
            
            # 随机参数范围 Random parameter ranges
            'random_time_range': (1.0, 3.0), # 随机时间范围(秒) Random time range in seconds
            'random_steps_range': (3, 8),     # 随机步数范围 Random steps range
            'random_angle_range': (30, 90),   # 随机角度范围(度) Random angle range in degrees
            
            # 动作参数 Action parameters
            'step_interval_s': 0.5,          # 步间隔时间(秒) Step interval in seconds
            'robot_speed_level': 3,          # 机器人速度等级(1-5) Robot speed level (1-5)
            
            # 速度标定参数 Speed calibration parameters
            'use_speed_calibration': True,   # 是否使用速度标定 Whether to use speed calibration
            'speed_tolerance': 0.05,         # 速度容差(m/s) Speed tolerance in m/s
        }
        
        # 速度档位标定数据 Speed level calibration data
        # 基于实际标定的精确数值 Accurate values based on actual calibration
        # Default values based on estimation, recommend using calibrate_speed_levels() for actual calibration
        self.speed_calibration = {
            1: 0.060, # 档位1对应0.060m/s (最慢) Level 1: 0.060m/s (slowest)
            2: 0.060, # 档位2对应0.060m/s Level 2: 0.060m/s
            3: 0.076, # 档位3对应0.076m/s (默认) Level 3: 0.076m/s (default)
            4: 0.092, # 档位4对应0.092m/s Level 4: 0.092m/s
            5: 0.106, # 档位5对应0.106m/s (最快) Level 5: 0.106m/s (fastest)
        }
    
    def get_config(self) -> tuple:
        """
        获取当前配置
        Get current configuration
        
        Returns:
            tuple: (success: bool, message: str, data: dict)
                - success: 获取是否成功 Whether get succeeded
                - message: 操作结果信息 Operation result message
                - data: 当前配置数据 Current configuration data
                
        Example:
            success, message, config_data = config_manager.get_config()
            if success:
                print(f"当前配置: {config_data}")
            else:
                print(f"获取配置失败: {message}")
        """
        try:
            config_data = {
                'default_config': self.default_config.copy(),
                'calibration_data': self._get_calibration_data(),
                'speed_calibration': self.speed_calibration.copy()
            }
            
            return True, "配置获取成功 Configuration retrieved successfully", config_data
            
        except Exception as e:
            return False, f"获取配置失败: {str(e)} Failed to get configuration: {str(e)}", {}
        
        # 重复的配置定义已移除，使用__init__中的统一配置
        # Duplicate configuration definition removed, using unified config from __init__
    
    def update_config(self, **kwargs) -> tuple:
        """
        更新全局配置参数
        Update global configuration parameters
        
        Args:
            **kwargs: 配置参数 Configuration parameters
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                - success: 更新是否成功 Whether update succeeded
                - message: 操作结果信息 Operation result message
                - data: 更新后的配置数据 Updated configuration data
                
        Example:
            success, message, config_data = config_manager.update_config(default_speed_ms=0.3)
            if success:
                print(f"配置更新成功: {message}")
            else:
                print(f"配置更新失败: {message}")
        """
        try:
            if not kwargs:
                return False, "没有提供配置参数 No configuration parameters provided", self.default_config.copy()
            
            self.default_config.update(kwargs)
            # 配置已更新 Configuration updated
            
            message = f"成功更新{len(kwargs)}个配置参数 Successfully updated {len(kwargs)} configuration parameters"
            return True, message, self.default_config.copy()
            
        except Exception as e:
            return False, f"更新配置失败: {str(e)} Failed to update configuration: {str(e)}", self.default_config.copy()
    
    def update_calibration(self, calibration_distance_cm: float, calibration_steps: int) -> tuple:
        """
        更新校准数据
        Update calibration data
        
        Args:
            calibration_distance_cm: 校准距离(厘米) Calibration distance in cm
            calibration_steps: 校准步数 Calibration steps
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                - success: 更新是否成功 Whether update succeeded
                - message: 操作结果信息 Operation result message
                - data: 更新后的校准数据 Updated calibration data
                
        Example:
            success, message, calib_data = config_manager.update_calibration(90, 10)
            if success:
                print(f"校准更新成功: {message}")
            else:
                print(f"校准更新失败: {message}")
        """
        try:
            if calibration_distance_cm <= 0:
                return False, "校准距离必须大于0 Calibration distance must be greater than 0", self._get_calibration_data()
            
            if calibration_steps <= 0:
                return False, "校准步数必须大于0 Calibration steps must be greater than 0", self._get_calibration_data()
            
            self.calibration_distance_cm = calibration_distance_cm
            self.calibration_steps = calibration_steps
            self.step_distance_cm = calibration_distance_cm / calibration_steps
            self.step_distance_m = self.step_distance_cm / 100.0
            
            # 校准数据已更新 Calibration data updated
            
            message = f"校准数据更新成功: {calibration_steps}步={calibration_distance_cm}cm Calibration data updated: {calibration_steps} steps = {calibration_distance_cm}cm"
            return True, message, self._get_calibration_data()
            
        except Exception as e:
            return False, f"更新校准数据失败: {str(e)} Failed to update calibration data: {str(e)}", self._get_calibration_data()
    
    def _get_calibration_data(self) -> Dict[str, Any]:
        """
        获取当前校准数据
        Get current calibration data
        
        Returns:
            dict: 校准数据字典 Calibration data dictionary
        """
        return {
            'calibration_distance_cm': self.calibration_distance_cm,
            'calibration_steps': self.calibration_steps,
            'step_distance_cm': self.step_distance_cm,
            'step_distance_m': self.step_distance_m
        }
    
    def _calculate_steps_from_distance(self, distance_m: float) -> tuple:
        """
        根据距离计算步数
        Calculate steps from distance
        
        Args:
            distance_m: 距离(米) Distance in meters
            
        Returns:
            tuple: (success: bool, message: str, data: int)
                success: 计算是否成功 Whether calculation was successful
                message: 计算结果消息 Calculation result message
                data: 步数 Number of steps
        
        Examples:
            success, msg, steps = config_manager._calculate_steps_from_distance(1.0)
        """
        try:
            if distance_m <= 0:
                return False, "距离必须大于0 Distance must be greater than 0", 0
            
            steps = max(1, int(distance_m / self.step_distance_m))
            return True, f"根据距离{distance_m:.2f}m计算出{steps}步 Calculated {steps} steps from {distance_m:.2f}m distance", steps
        except Exception as e:
            return False, f"计算步数失败: {str(e)} Failed to calculate steps: {str(e)}", 0
    
    def _calculate_step_interval_from_speed(self, speed_ms: float) -> tuple:
        """
        根据速度计算步间隔
        Calculate step interval from speed
        
        Args:
            speed_ms: 速度(米/秒) Speed in m/s
            
        Returns:
            tuple: (success: bool, message: str, data: float)
                success: 计算是否成功 Whether calculation was successful
                message: 计算结果消息 Calculation result message
                data: 步间隔时间(秒) Step interval in seconds
        
        Examples:
            success, msg, interval = config_manager._calculate_step_interval_from_speed(0.5)
        """
        try:
            if speed_ms <= 0:
                return False, "速度必须大于0 Speed must be greater than 0", 0.5
            
            steps_per_second = speed_ms / self.step_distance_m
            step_interval = 1.0 / max(0.1, steps_per_second)  # 防止除零 Prevent division by zero
            return True, f"根据速度{speed_ms:.3f}m/s计算出步间隔{step_interval:.3f}s Calculated step interval {step_interval:.3f}s from speed {speed_ms:.3f}m/s", step_interval
        except Exception as e:
            return False, f"计算步间隔失败: {str(e)} Failed to calculate step interval: {str(e)}", 0.5
    
    def _select_optimal_speed_level(self, target_speed_ms: float) -> tuple:
        """
        根据目标速度选择最接近的速度档位
        Select the closest speed level for target speed
        
        Args:
            target_speed_ms: 目标速度(米/秒) Target speed in m/s
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 选择是否成功 Whether selection was successful
                message: 选择结果消息 Selection result message
                data: 包含最佳档位和实际速度的字典 Dict with optimal level and actual speed
        
        Examples:
            success, msg, result = config_manager._select_optimal_speed_level(0.3)
            if success:
                level = result['level']
                speed = result['speed']
        """
        try:
            if target_speed_ms <= 0:
                return False, "目标速度必须大于0 Target speed must be greater than 0", {'level': 3, 'speed': 0.25}
            
            if not self.default_config['use_speed_calibration']:
                # 如果不使用速度标定，返回默认档位
                default_level = self.default_config['robot_speed_level']
                return True, f"未启用速度标定，使用默认档位{default_level} Speed calibration disabled, using default level {default_level}", {
                    'level': default_level,
                    'speed': target_speed_ms
                }
            
            best_level = 3  # 默认档位 Default level
            best_speed = self.speed_calibration[3]
            min_diff = abs(best_speed - target_speed_ms)
            
            # 寻找最接近的速度档位 Find the closest speed level
            for level, actual_speed in self.speed_calibration.items():
                diff = abs(actual_speed - target_speed_ms)
                if diff < min_diff:
                    min_diff = diff
                    best_level = level
                    best_speed = actual_speed
            
            return True, f"为目标速度{target_speed_ms:.3f}m/s选择档位{best_level}(实际{best_speed:.3f}m/s) Selected level {best_level} (actual {best_speed:.3f}m/s) for target speed {target_speed_ms:.3f}m/s", {
                'level': best_level,
                'speed': best_speed,
                'speed_diff': min_diff
            }
        except Exception as e:
            return False, f"选择最优速度档位失败: {str(e)} Failed to select optimal speed level: {str(e)}", {'level': 3, 'speed': 0.25}
    
    def update_speed_calibration(self, calibration_data: Dict[int, float]) -> tuple:
        """
        更新速度标定数据
        Update speed calibration data
        
        Args:
            calibration_data: 档位->实际速度的映射 Level->actual_speed mapping
            
        Returns:
            tuple: (success: bool, message: str, data: Dict[int, float])
                - success: 更新是否成功 Whether update succeeded
                - message: 操作结果信息 Operation result message
                - data: 更新后的速度标定数据 Updated speed calibration data
                
        Example:
            new_calibration = {1: 0.05, 2: 0.08, 3: 0.12, 4: 0.15, 5: 0.20}
            success, message, speed_data = config_manager.update_speed_calibration(new_calibration)
        """
        try:
            if not calibration_data:
                return False, "没有提供速度标定数据 No speed calibration data provided", self.speed_calibration.copy()
            
            # 验证数据格式 Validate data format
            for level, speed in calibration_data.items():
                if not isinstance(level, int) or level < 1 or level > 5:
                    return False, f"无效的速度档位: {level}，必须是1-5之间的整数 Invalid speed level: {level}, must be integer between 1-5", self.speed_calibration.copy()
                
                if not isinstance(speed, (int, float)) or speed <= 0:
                    return False, f"无效的速度值: {speed}，必须是大于0的数值 Invalid speed value: {speed}, must be positive number", self.speed_calibration.copy()
            
            # 更新速度标定数据 Update speed calibration data
            self.speed_calibration.update(calibration_data)
            
            message = f"成功更新{len(calibration_data)}个速度档位的标定数据 Successfully updated speed calibration for {len(calibration_data)} levels"
            return True, message, self.speed_calibration.copy()
            
        except Exception as e:
            return False, f"更新速度标定数据失败: {str(e)} Failed to update speed calibration: {str(e)}", self.speed_calibration.copy()
    
    def calibrate_speed_levels(self) -> tuple:
        """
        自动标定速度档位
        Automatically calibrate speed levels
        
        通过实际测试来标定各个速度档位的真实速度
        Calibrate actual speeds for each speed level through real testing
        
        Returns:
            tuple: (success: bool, message: str, data: dict)
                - success: 标定是否成功 Whether calibration succeeded
                - message: 标定结果信息 Calibration result message
                - data: 标定结果数据 Calibration result data
                
        Note:
            这是一个模拟实现，实际使用时需要连接真实硬件进行测试
            This is a simulation implementation, real hardware testing is needed in practice
        """
        try:
            if self.debug:
                print("[DEBUG] 开始自动速度标定 Starting automatic speed calibration")
            
            # 模拟标定过程 Simulate calibration process
            # 在实际实现中，这里应该控制机器人以不同速度档位移动，并测量实际速度
            # In real implementation, this should control robot movement at different speed levels and measure actual speeds
            
            calibration_results = {}
            test_distance_m = 1.0  # 测试距离1米 Test distance 1 meter
            
            for level in range(1, 6):
                if self.debug:
                    print(f"[DEBUG] 标定速度档位 {level} Calibrating speed level {level}")
                
                # 模拟测试时间 Simulate test time
                # 实际实现中应该测量机器人移动指定距离所需的时间
                # In real implementation, should measure time needed for robot to move specified distance
                simulated_time_s = random.uniform(8.0, 15.0)  # 模拟8-15秒的移动时间
                actual_speed_ms = test_distance_m / simulated_time_s
                
                calibration_results[level] = round(actual_speed_ms, 3)
                
                if self.debug:
                    print(f"[DEBUG] 档位 {level}: 实际速度 {actual_speed_ms:.3f} m/s Level {level}: actual speed {actual_speed_ms:.3f} m/s")
                
                # 模拟测试间隔 Simulate test interval
                time.sleep(0.1)
            
            # 更新速度标定数据 Update speed calibration data
            self.speed_calibration.update(calibration_results)
            
            message = f"速度标定完成，已更新5个档位的标定数据 Speed calibration completed, updated calibration data for 5 levels"
            
            return True, message, {
                'calibration_results': calibration_results,
                'test_distance_m': test_distance_m,
                'updated_speed_calibration': self.speed_calibration.copy()
            }
            
        except Exception as e:
            return False, f"自动速度标定失败: {str(e)} Automatic speed calibration failed: {str(e)}", {}